Motion smoothing in 3-d position sensing apparatus

ABSTRACT

A method of estimating and extrapolating the position of an article is provided. The article&#39;s position is detected by relatively infrequent ultrasonic ranging, and provides more frequent reports from internal accelerometers, gyroscopes and optional magnetometers. In a first instance, the method includes calculating new position, velocity and orientation vectors by linearly interpolating between the readings of the sensors at two times. In a second instance, the method includes estimating the orientation of the article by calculating the duration of a timeslice, making a rotational increment matrix and taking the product of the initial orientation and the increment matrix for the appropriate number of timeslices. In a third instance, the method includes calculating the acceleration of an article between three ultrasonically determined locations and recalibrating the accelerometers to align the measured acceleration with the calculated acceleration.

CROSS-REFERENCE TO RELATED APPLICATIONS

This application is a continuation of and claims the benefit ofInternational Application Number PCT/GB2009/000873 filed Apr. 2, 2009,which is hereby incorporated herein by reference in its entirety.

FIELD OF THE INVENTION

This invention relates to continuous positional and orientationaltracking of an object in three dimensions, and optionally, the use oftracking data to control an onscreen pointer.

DESCRIPTION OF THE RELATED ART

There is a desire to be able to accurately and continuously track theposition of a handheld controller of the type described inWO-A-2007/003126, in conjunction with suitable software. The prior arthardware is limited in that position is given less frequently thandesired. For example, a gaming application may require a positionalupdate 60 times a second in view of the expected frame rate of thegaming applications. In practice a typical minimum repeat period forultrasonic measurements is of the order of 33 to 132 ms. Thus there is aneed to use other sources of information to accurately fill in the databetween these known positions.

BRIEF SUMMARY OF EMBODIMENTS OF THE INVENTION

In accordance with a first aspect of the invention there is provided amethod of estimating position and orientation of an article comprisingreading the orientation of the article from three gyroscopes mounted onthe article and reading the acceleration of the article from threeaccelerometers mounted on the article and further comprising estimatinga continuous position (vector P), velocity (vector V) and orientation(matrix O) of an article at time T, between T and an initial time T₀where: initial position vector=P₀; initial velocity vector=V₀; initialorientation matrix=O₀; initial rotational velocity, relative to thearticle, vector=R₀; initial acceleration, relative to the article,vector=A₀ and comprising re-calculating (vector P), velocity (vector V)and orientation (matrix O) at a subsequent time T₁ using new informationfrom the accelerometers and gyroscopes where: new acceleration relativeto the object vector=A₁; new rotational velocity relative to the objectvector=R₁; the re-calculation being carried out by, linearlyinterpolating from R₀ to R₁ to discover the local rotational velocity atany time between T₀ and T₁, and by linearly interpolating from A₀ to A₁to discover the local acceleration.

Optionally, the article may further comprise three magnetic sensorsmounted therein, the method further comprising estimating a continuousposition (vector P), velocity (vector V) and orientation (matrix O) ofan article at time T, between T and an initial time T₀ where: initialdirection of fixed magnetic field, relative to the article, vector=F₀(in angular units) and comprising re-calculating (vector P), velocity(vector V) and orientation (matrix O) at a subsequent time T₁ using newinformation from the

accelerometers, gyroscopes and magnetometers where: new direction offixed magnetic field, relative to the article, vector=F₁ (in angularunits); the re-calculation being carried out by linearly interpolatingfrom F₀ to F₁ to discover the local direction of fixed magnetic field.

According to a second aspect of the invention there is provided a methodof estimating position and orientation of an article comprisingextrapolating a position (vector P), and orientation (matrix O) of thearticle at time T, moveable from an initial state at time T₀, describedby : initial position vector P₀, initial velocity vector V₀, initialorientation matrix O₀ (in angular units), and initial rotationalvelocity, relative to the article, vector R₀, further comprisingcalculating the duration of a timeslice T_(Slice)=(T−T₀)/N, where Ntends to zero, making a rotational increment matrix (M) to create arotation matrix from a rotation expressed as a vector of componentsrepresenting x, y, and z axes of rotation :M=MatrixCreate(T_(Slice)*R₀), where the estimated orientation andestimated position are given as O=M1×M2×M3×. . . ×MN×O0 and P=P₀+V(T−T₀)respectively.

Optionally, either method may further comprise using ultrasonic readingsto correct the velocity estimates and thereby estimated position, bymodifying the historical velocity estimate when each new ultrasonicreading is received, to make the estimated position match the positiongiven by the new ultrasonic measurement.

According to a third aspect of the invention there is provided a methodof estimating position and orientation of an article comprisingcorrecting orientation, and optionally velocity, by taking a pluralityof readings from the output of three accelerometers mounted on thearticle, using three positions given by ultrasonic time-of-f lightmeasurements, during transit between the first, second and thirdultrasonic-derived positions, assuming that acceleration of the articlebetween the first to the third the positions was substantially constantand verifying this against the plurality of accelerometer readings,calculating the acceleration vector at the second the position using theposition and time information about the three known positions andcomparing that calculated acceleration vector with one obtained from theaccelerometers, and thereby re-calibrating the measurements taken fromthe accelerometers.

In other aspects, the invention provides a computer program productcarrying program steps which when executed on suitable hardware causesthe hardware to carry out the steps of the any of the claimed methodsand a method of controlling an on-screen pointer, comprising using theposition and orientation information of the article derived using anypreceding claim, and determining where a line through a predeterminedone of the article's axes intersects a plane, which plane may be theplane of the screen.

Position may be sensed using ultrasonic time-of-flight measurementsbetween fixed and mobile components (such as a handheld gamescontroller), with the readings supplemented by interim readings fromaccelerometers and gyroscopes. Optionally, magnetometers may be used toprovide an absolute directional reference based on the earth's magneticfield or an artificially generated field. The ultrasonic measurementmay, for example, be as described in our co-pending WO-A- 2007/003126,the disclosure of which is incorporated herein by reference.

In taking interim readings from accelerometers and gyroscopes (andoptionally magnetometers), it is necessary to ascertain the orientationof the controller in order that the on-board sensors can be interpretedcorrectly—such orientation knowledge also provides a second application;that of achieving control of an onscreen pointer with the controller.

In summary, the invention allows determination of complete real-timemovement and orientation of a hand-held controller. The system is ableto ascertain motion and orientation, provided by preferably 3 orthogonalaccelerometers and preferably 3 orthogonal gyroscopes (and optionallymagnetometers) respectively, to describe the motion, but with constantfeedback/correction from every reliable ultrasonic position received.Thus the subsequent motion is governed by the ultrasonic positions, withthe inertial guidance of the accelerometers and gyroscopes filling inthe motion for the microseconds in-between ultrasonic position readings.

The small mismatches between the predicted position, the actual nextultrasonic position, and the extraction of the underlying motion fromjust ultrasonic readings, are used to calibrate the electroniccomponents of the system, in an attempt to reduce errors to zero. Inaddition, this live-calibration substantially eliminates linear velocitydrift from the accelerometers, and rotational drift from the gyroscopesenabling accurate knowledge of the controller's rotation and thusoperation as a pointer. Such feedback also allows complete recovery fromany initial conditions or an errored state.

BRIEF DESCRIPTION OF THE DRAWINGS

Embodiments of the invention will now be described by way of example andwith reference to the drawings in which:

FIG. 1 is a schematic diagram showing the estimation problem;

FIG. 2 is a schematic diagram showing extrapolation of a trajectory;

FIG. 3 is a schematic diagram showing correction of historicalextrapolation estimates;

FIG. 4 is a schematic diagram showing correction of historicalpositional estimates after a velocity correction;

FIG. 5 is a schematic diagram showing calculation of an accelerationvector from three ultrasonic position readings; and

FIG. 6 is a schematic diagram showing recalculated orientation followingaccelerometer re-calibration.

DETAILED DESCRIPTION OF THE EMBODIMENTS OF THE INVENTION

The term “gyroscope”, and derivatives thereof, is used herein to referto orientation detecting means having a similar function to a gyroscopeor gyroscopic sensor. This term is used to include, but is in no meanslimited to, devices such as gyroscopes; fiber optic gyroscopes; lasergyroscopes; vibrating structure gyroscopes and magnetohydrodynamicsensors. The term is intended to encompass a single such detecting meansor group thereof acting jointly or independently. This term is also usedto refer to pairs accelerometers arranged to produce similar readings tothose of gyroscopes, which may be through an electronic or softwareinterface. In particular, where three gyroscopes are referred to incombination in order to provide rotational information about an articlearound three non-parallel axes, the skilled man will realize thatdifferent gyroscopic sensors may he able to produce the desiredinformation from a greater or smaller number of individual sensors. Forexample, whilst three vibrating structure gyroscopes may be required toprovide complete rotational information in three dimensions, a single“traditional” gyroscope may be able to provide all of this information.Furthermore, the skilled man will realize that while some gyroscopesprovide orientation data and others rotational acceleration data, thesetwo data may be interconverted, so allowing the requirements of theinvention to be fulfilled.

A handheld controller or mobile component of the type described in WO-A-2007/003126 is augmented with sensors able to measure orientation andacceleration in three orthogonal axes such as three, preferablyorthogonally mounted, gyroscopes and three, preferably orthogonallymounted, accelerometers. These extra sensors can be polled more rapidlythan the ultrasonic measurement system and by tracking relative movementof the controller, allow interpolation and extrapolation of theultrasonic measurements of position and also allow orientation of thecontroller to be measured. The gyroscopes may he supplemented withmagnetometers to further improve orientation sensing.

The system seeks to calculate substantially continuous position andorientation of an object such as a handheld games controller or pointer,here referred to as “mobile component” from the following discreteinformation:

-   -   Approximate position of a mobile component every 33-132 ms, by        triangulating the times of flight of an ultrasonic pulse        travelling from the component to 3 or more sensors of known        position, separated by 20 cm or more from each other.    -   Approximate resultant acceleration of the mobile component,        every 11 ms, by reading 3 accelerometers attached to the mobile        component and oriented at preferably at 90 degrees to each other        and necessarily non-parallel to one another. This acceleration        is relative to the mobile component, because the accelerometers        are mounted on the component itself.    -   Approximate rotational velocity of the component, every 11 ms,        by reading 3 gyroscopes mounted to the mobile component        preferably at 90 degrees to each other and necessarily        non-parallel to one another. This rotational velocity is        relative to the component, because the gyroscopes are mounted on        the component itself.    -   Optionally, the rotational offset of the mobile component from        the vector of the local magnetic field, whether it is that of        the Earth, or artificially induced, every 11 ms, by reading 3        magnetometers preferably at 90 degrees to each other and        necessarily non-parallel to one another, again mounted on the        component.

It will be appreciated that all the values above are necessarilyapproximate because the hardware will be affected by noise, and may alsosuffer from errors in alignment, or errors from incorrect centering andcalibration. It is therefore preferable that the method described belowis robust and stable, even given the imprecise nature of its inputs.

In addition, the method is preferably able to recover from a completeabsence of data, and return rapidly to the correct position andorientation.

The method also allows any data that is assessed to be unreliable to bedisregarded without undue error or instability.

FIG. 1 shows the core problem. Relatively infrequent ultrasonicpositional data points 2, are relatively reliable in terms of accuracy.More frequent updates 4 from the other hardware components(accelerometers, gyroscopes and optional magnetometers), which cancontribute information to the controller's situation are also available.However, instead of simply knowing the object's position at thepositions 2, the problem demands being able to calculate it at anyintermediate points on the line 6 which interconnects the points 2 and4.

Using the methods described below, the position and orientation can betracked, extrapolated beyond the last data input, enhanced when data isreceived and fed back with a number of corrections to counter drillingand error that is inherent in real hardware applications. The methodsmay be used separately, or preferably, in combination.

Method 1 Overview

This method allows the estimation of a continuous position (vector P),and orientation (matrix O) of an article at time T, moveable from aninitial situation at time T0, described by :

1. position, (vector P₀ in meters)

2. velocity, (vector V₀ in meters/second)

3. orientation (matrix O₀ in radians)

4. rotational velocity relative to the object (vector R₀ inradians/second)

With reference to FIG. 2, this method is used to extrapolate the line 6onwards from the last data input received from the controller. On thediagram, this extrapolation is marked by circle 8. This line representsan estimate of the trajectory of the mobile component which is beingtracked.

This method is now described in more detail.

Initially we assume zero linear and rotational acceleration over theintervening time between T₀ and T. However, this method may be extendedto accommodate these initial acceleration values being non-zero.

For simplicity, we detail updating of the orientation, by incrementingin small time slices. The smaller the slices are made, the more accuratethe method becomes and, ultimately mathematical integration may be usedas the size of time slice tends to zero if needed. We choose tosubdivide the time period (T−T₀) into N time slices, where, in apreferred embodiment, N is a chosen whole number giving each time slicea duration of approximately 10 ms.

Duration of the timeslice is calculated : T_(Slice)=(T−T₀)/N

Make a rotational increment matrix (M) using a standard function(MatrixCreate) such as the Microsoft® DirectX® 9 functionD3DXMatrixRotationYawPitchRoll, to create a rotation matrix from arotation expressed as a vector of components representing x, y, and zaxes of rotation :

M=MatrixCreate(T_(Slice)*R₀)

O=Mi×M2×M3×. . . (with N instances of M) . . . ×O₀

P=P₀+V(T−T₀)

This provides a representation of an extrapolated portion of the line 8which may be fed back to an application seeking positional informationsince the last accelerometer/gyroscope/magnetometer reading 4.

Method 2 Overview

This method allows estimation of a continuous position (vector P),velocity (vector V) and orientation (matrix O) of an article at time T,which is between (and including) an initial time T₀ with its situationdescribed by :

1. position, (vector P₀ in meters)

2. velocity, (vector V₀ in meters/second)

3. orientation (matrix O₀ in radians)

4. rotational velocity relative to the object (vector R₀ inradians/second)

5. acceleration relative to the object (vector A₀ in meters/seconds²)

6. optional direction of fixed magnetic field, relative to the object(vector F₀ in radians)

and a subsequent time T₁ at which time the following additionalinformation becomes known, by virtue of new data from theaccelerometers, gyroscopes and magnetometers :

1. acceleration relative to the object (vector A₁ in meters/seconds²)

2. rotational velocity relative to the object (vector R₁ inradians/second)

3. direction of fixed magnetic field, relative to the object (vector F₁in radians)

With reference to FIG. 3, this method is used to amend the precedingmotion data when a new batch of accelerometer/gyroscope/magnetometerdata comes in. This can be used retrospectively to correct theextrapolation derived in the method described above. Each of the partsof the line marked by circles 10 was established via this method;corrected from their previously extrapolated values which are indicatedby broken lines 12.

This method allows calculation of the situation at time T₁, which canthen be used as the initial conditions (at time T₀) for the first methodabove.

This method is now described in detail.

Again for simplicity, we consider updates over a small timeslice. Againwe choose to subdivide the time period (T−T₀) into N time slices, whereN is a whole number typically giving each time slice a duration ofapproximately 10 ms.

From R₀ and R₁, we linearly interpolate to discover the local (relativeto object) rotational velocity at any time between T₀ and T₁. Similarlywe linearly interpolate the local acceleration, and direction of fixedmagnetic field.

We iterate through the N time slices. In practice this is desirable inorder to save out historical data, for processing by the end applicationof the controller. For each time-slice one can see that the initial datais available from the initial conditions for the first one, and thensubsequently from the endpoint of the prior time-slice.

Duration of the time-slice is calculated : T_(Slice)=(T−T₀)/N

Increment rotation velocity per half slice : R_(Half) _(—) _(inc)=¼*(R₀)* T_(Slice);

Increment acceleration per half slice : A_(Half) _(—)_(inc)=¼*(R₀+R₁)*T_(Slice).

Increment magnetic field per half slice : F_(Half)_(inc)=¼*(F₀+F₁)*T_(Slice); So, for each of N time-slices :

(We preferably use values in the middle of the time-slices as the bestrepresentation of the values across the whole time-slice.)

R _(MidSlice) =R _(StartSlice) +R _(Half) _(—) _(inc)

R _(EndSlice) =R _(MidSlice) +R _(Half) _(—) _(inc)

A _(MidSlice) =A _(StartSlice) +A _(Half) _(—) _(inc)

A _(EndSlice) =A _(MidSlice) +A _(Half) _(—) _(inc)

F _(MidSlice) =F _(StartSlice) +F _(Half) _(—) _(inc)

F _(EndSlice) =F _(MidSlice) +F _(Half) _(—) _(inc)

Increment rotational matrix for half a time slice :M=MatrixCreate(R_(MidSlice))

O _(MidSlice) =M*O _(StartSlice)

The direction of the true magnetic field F_(True) is known, and we havea direction given by F_(MidSlice) that should match it, provided wetransform F_(MidSlice) back into world space (it's relative to thecontroller at present).

Hence we rotate O_(MidSlice) by the shortest rotation such that(O_(MidSlice) ⁻¹* F_(MidSlice)) is aligned with F_(True);

O _(EndSlice) =M*O _(MidSlice)

Similarly we correct O_(EndSlice) by reference to rotating by theshortest rotation such that (O_(MidSlice) ⁻¹*F_(EndSlice)) is alignedwith F_(True);

Now calculate average world acceleration A_(w) for this slice from theaverages.

A _(w) =O _(MidSlice) ×A _(MidSlice)

So use this acceleration in the equations of motion for this slice.

P _(EndSlice) =P _(StartSlice)+(V _(StartSlice) *T _(Slice))+(½*T_(Slice) ²)

V _(EndSlice) =V _(StartSlice)+(A _(w) *T _(Slice))

Finally, we read off the values from the end of Nth slice to give us P,V, and O.

Method 3 Overview

This method uses ultrasonic readings to correct linear velocityestimates, and thus position.

Every time a new ultrasonic reading is received, it is treated as thecorrect location, and the historical velocity estimate of the object ismodified in such a way that the tracked position becomes correct at thispoint in time.

With reference to FIG. 4, the dotted path 14 represents the path whichwas being tracked, complete with its interim corrections 4 from the morefrequent accelerometer/gyroscope/magnetometer updates, and even anextrapolation up until the point of our most recent position. Now, whenthe ultrasonic position 2′ is received, we adjust the velocity at thetime of the last ultrasonic reading 2″, such that the subsequent motionputs our tracked position at exactly the position the ultrasonics arenow telling us. As a consequence of this velocity correction, we willsee that the position is also corrected and the assumed positions of theaccelerometer/gyroscope/magnetometer updates are adjusted to newpositions 4-1.

The method is now described in more detail.

A new ultrasonic position (vector P), is received at time T. Using themethods of tracking the object which are described above, we calculateour position to be vector P₀.

Historically, we received our last ultrasonic position at time T0, andsince we are using this method where calculated and ultrasonic positionsare synchronized, we can assume that this was the case at time T0.

Therefore in time (T−T₀), our position has deviated by (P₀−P), and wecalculate the extra velocity we should have had since time T₀ in orderfor that deviation to be zero.

V _(Increment)=(P−P ₀)/(T−T ₀)

This velocity increment propagates through the historical data to thepresent moment, yielding a current velocity that is increased by theincrement and a new calculated P₀ that exactly matches P.

Because it is a velocity change, the position curve remains continuous.However, it can be noted that there will be a step/discontinuity in thevelocity at time T₀, which could be post-processed if continuity isrequired in the historical velocity data, as well as smoothness in thehistorical position data.

Method 4 Overview

This method corrects orientation, and optionally velocity, by taking aplurality of readings from the output of three accelerometers mounted atpreferably at 90 degrees to each other and necessarily non-parallel toone another on the article, during transit between the three knownpositions, assuming that acceleration of the article between the firstto the third known positions was substantially constant and verifyingthis against the plurality of accelerometer readings, calculating theacceleration vector at the second known position using the position andtime information about the three known positions and comparing thatcalculated acceleration vector with the one obtained from theaccelerometers (once rotated back into world space from the local spaceof the article).

With reference to FIG. 5, 3 ultrasonic positions 2 are given with theirassociated times. An acceleration vector 16 is calculated from thatdata, and an estimated vector 18 is based on accelerometer readings. Anorientation correction 20 is then given to align these two vectors 16and 18 which should represent the same thing.

The orientation change to correct vector 18 to match vector 16 iscalculated and applied to historical calculations from the time of thesecond ultrasonic reading 2′″. This reading is chosen because it can beconfirmed via former and later ultrasonic readings, whether theassumption of constant accelerations holds true. Therefore thesubsequent path of the tracked article can be recalculated withcorrected accelerometer readings, from the time of T₂ to the present—seeFIG. 6.

From our calculations, we can also ascertain velocity at time T₂, whichcan be optionally fed into the calculations.

This method is now described in more detail.

Firstly we calculate the acceleration and velocity at T₂, using formulaederived from simultaneous equations of linear motion and the positionvectors P₁, P², and P and the times T₁, T₂, and T.

Position change between P₁ and P₂, P_(A)=P₂−P₁

Position change between P and P₁, P_(B)=P−P₂

Time change between P₁ and P₂, T_(A)=T₂−T₁

Time change between P and P₁, T_(B)=T−T₁

Then acceleration at T₂,A₂=(2*((T_(A)*P_(B))−(T_(B)*P_(A))))/(T_(A)*T_(B)*(T_(A)+T_(B)))

And velocity at T₂, V₂=(P_(A)/T_(A))+(½*A₂*T_(A))

Next we look at the accelerometer readings from time T₂, to give us theacceleration in the local frame of the controller, which must then berotated by the inverse of the orientation matrix we have at that time.Thus we get A_(2accel).

We compare the magnitudes of A₂ and A_(2accel), to check they areconsistent, i.e. say within 10%.

If so, then we adjust the orientation at time T₂ to align A_(2accel)with A₂.

Optionally we may also choose to set the velocity at time T₂ to V₂.

Then recalculate our article's situation over the time, re-feeding inthe inputs between T₂ and current time T, to arrive at a correctedhistory and current situation of the article.

It will be understood that preferably, and for maximum accuracy ofestimates, these various methods are used together. However, they may beused separately, or alternatively in different combinations with eachother.

Computer Program

The methods typically will be implemented in software with connectionsto hardware accelerometers, gyroscopes, magnetometers, and ultrasonictransducers. Thus the invention encompasses a computer program product,such as downloadable, software, firmware or a physical carrier such as aDVD carrying program steps which when executed on suitable hardwarecauses the hardware to carry out the steps of the claimed methods.

Smoothing

The resulting measured motion of the mobile component will be subject toa degree of perturbation as a result of the level of noise and errorpresent in the system. These perturbations may be smoothed by a varietyof techniques. For example, values may be blended with a proportion oftheir previous value and a proportion of their new value, yielding asmoothness that takes into account past values whose significancediminishes over time. Such a smoothing method can be further refined bymaking those proportions themselves proportional to the differencebetween new and old values, thus large differences may force a faster oreven instant adoption of the new value, whereas small differences allowa more gradual integration of new values, promoting smoothness. Thisachieves a system that is both smooth, and responsive to change.

A Pointer

Since the complete motion and orientation. of the object is described,the user may ascertain where a line through one of the object's axesintersects an arbitrary fixed plane, in effect allowing the object toact as a pointing device.

1. A method of estimating position and orientation of an article,comprising: reading the orientation of the article from at least threegyroscopes mounted on the article; reading the acceleration of thearticle from three accelerometers mounted on the article; at time Testimating a position vector velocity vector V, and orientation matrix Oof an article, between T and an initial time T₀ where: an initialposition vector=P₀, an initial velocity vector=V₀, an initialorientation matrix=O₀, an initial rotational velocity, relative to thearticle, vector=R₀, and an initial acceleration, relative to thearticle, vector=A₀; and re-calculating the position vector P, velocityvector V, and orientation matrix O at a subsequent time T₁ using newinformation from the accelerometers and gyroscopes where: newacceleration relative to the object vector=A₁, new rotational velocityrelative to the object vector=R₁,and the re-calculation is carried outby, linearly interpolating from R₀ to R₁ to discover the localrotational velocity at any time between T₀ and T₁, and by linearlyinterpolating from A₀ to A₁ to discover the local acceleration.
 2. Amethod according to claim 1, the article further comprising threemagnetic sensors mounted therein, and the method further comprising: attime T estimating the position vector P, velocity vector V, andorientation matrix O of an article, between T and an initial time T₀where: initial direction of fixed magnetic field, relative to thearticle, vector=F₀ (in angular units); and re-calculating the positionvector P, velocity vector V, and orientation matrix O at a subsequenttime T using new information from the accelerometers, gyroscopes andmagnetometers where: new direction of fixed magnetic field, relative tothe article, vector=F₁ (in angular units), and the re-calculation iscarried out by linearly interpolating from F₀ to F₁ to discover thelocal direction of fixed magnetic field.
 3. A method according to claim1, further comprising using ultrasonic readings to correct the velocityestimates and thereby estimated position, by modifying the historicalvelocity estimate when each new ultrasonic reading is received, to makethe estimated position match the position given by the new ultrasonicmeasurement.
 4. A method of estimating position and orientation of anarticle, comprising: extrapolating a position vector P, and orientationmatrix O of the article at time T, moveable from an initial state attime T₀, described by: an initial position vector=P₀, an initialvelocity vector=V₀, an initial orientation matrix=O₀, (in angularunits), and initial rotational velocity, relative to the article,vector=R₀; calculating the duration of a timeslice T_(Slice)=(T−T₀)/N,where N tends to zero; and making a rotational increment matrix M tocreate a rotation matrix from a rotation expressed as a vector ofcomponents representing x, y, and z axes of rotation :M=MatrixCreate(T_(Slice)*R₀), where the estimated orientation andestimated position are given as O=M₁×M₂×M₃ ×. . . ×M_(N)×O₀ andP=P₀+V(T−T₀) respectively.
 5. A method according to claim 4, furthercomprising using ultrasonic readings to correct the velocity estimatesand thereby estimated position, by modifying the historical velocityestimate when each new ultrasonic reading is received, to make theestimated position match the position given by the new ultrasonicmeasurement.
 6. A method of estimating position and orientation of anarticle, the method comprising correcting orientation, and optionallyvelocity, by: taking a plurality of readings from the output of at leastthree accelerometers mounted on the article; using at least threepositions given by ultrasonic time-of-flight measurements, duringtransit between at least the first, second and third ultrasonic-derivedpositions, assuming that acceleration of the article between the firstto the third the positions was substantially constant and verifying thisagainst the plurality of accelerometer readings; calculating theacceleration vector at the second position using the position and timeinformation about the at least three known positions and comparing thatcalculated acceleration vector with one obtained from theaccelerometers; and thereby re-calibrating the measurements taken fromthe accelerometers.
 7. A computer program product embedded onnon-transitory computer-readable medium, the computer program productcarrying program steps which when executed on suitable hardware causesthe hardware to carry out the steps of: reading the orientation of thearticle from at least three gyroscopes mounted on the article; readingthe acceleration of the article from at least three accelerometersmounted on the article; at time T estimating a position vector P,velocity vector V, and orientation matrix O of an article, between T andan initial time T₀ where: an initial position vector=P₀, an initialvelocity vector=V₀, an initial orientation matrix=O₀), an initialrotational velocity, relative to the article, vector=R₀, and an initialacceleration, relative to the article, vector=A₀; and re-calculating theposition vector P, velocity vector V, and orientation matrix O at asubsequent time T₁ using new information from the accelerometers andgyroscopes where: new acceleration relative to the object vector=A₁, newrotational velocity relative to the object vector=R₁,and there-calculation is carried out by, linearly interpolating from R₀ to R₁to discover the local rotational velocity at any time between T₀ and T₁,and by linearly interpolating from A₀ to A₁ to discover the localacceleration.
 8. A computer program product according to claim 7, thearticle further comprising three magnetic sensors mounted therein, andthe steps further comprising: at time T estimating a position vector P,velocity vector V, and orientation matrix O of an article, between T andan initial time T₀ where : an initial direction of fixed magnetic field,relative to the article, vector=F₀ (in angular units); andre-calculating the position vector P, velocity vector V, and orientationmatrix O at a subsequent time T₁ using new information from theaccelerometers, gyroscopes and magnetometers where: a new direction offixed magnetic field, relative to the article, vector=F₁ (in angularunits), and the re-calculation is carried out by linearly interpolatingfrom F₀ to F₁ to discover the local direction of fixed magnetic field.9. A computer program product according to claim 7, the steps furthercomprising using ultrasonic readings to correct the velocity estimatesand thereby estimated position, by modifying the historical velocityestimate when each new ultrasonic reading is received, to make theestimated position match the position given by the new ultrasonicmeasurement.
 10. A computer program product embedded on non-transitorycomputer-readable medium, the computer program product carrying programsteps which when executed on suitable hardware causes the hardware tocarry out the steps of: extrapolating a position vector P, andorientation matrix O of the article at time T, moveable from an initialstate at time T₀, described by: an initial position vector=P₀, aninitial velocity vector=V₀, an initial orientation matrix=O₀, (inangular units), and an initial rotational velocity, relative to thearticle, vector=R₀; calculating the duration of a timesliceT_(Slice)=(T−T₀)/N, where N tends to zero; and making a rotationalincrement matrix M to create a rotation matrix from a rotation expressedas a vector of components representing x, y, and z axes of rotation :M=MatrixCreate(T_(Slice)*R₀), where the estimated orientation andestimated position are given as O=M₁×M₂×M₃ ×. . . ×M_(N)×O₀ andP=P₀+V(T−T₀) respectively.
 11. A computer program product according toclaim 10, the steps further comprising using ultrasonic readings tocorrect the velocity estimates and thereby estimated position, bymodifying the historical velocity estimate when each new ultrasonicreading is received, to make the estimated position match the positiongiven by the new ultrasonic measurement.
 12. A computer program productembedded on non-transitory computer-readable medium, the computerprogram product carrying program steps which when executed on suitablehardware causes the hardware to carry out the steps of: taking aplurality of readings from the output of at least three accelerometersmounted on the article; using at least three positions given byultrasonic time-of-flight measurements, during transit between at leastthe first, second and third ultrasonic-derived positions, assuming thatacceleration of the article between the first to the third the positionswas substantially constant and verifying this against the plurality ofaccelerometer readings; calculating the acceleration vector at thesecond the position using the position and time information about the atleast three known positions and comparing that calculated accelerationvector with one obtained from the accelerometers; and therebyre-calibrating the measurements taken from the accelerometers.
 13. Amethod of controlling an on-screen pointer, comprising: reading theorientation of the article from at least three gyroscopes mounted on thearticle; reading the acceleration of the article from at least threeaccelerometers mounted on the article; at time T estimating a positionvector P, velocity vector V, and orientation matrix O of an article,between T and an initial time T₀ where: an initial position vector=P₀,an initial velocity vector=V₀, an initial orientation matrix=O₀, aninitial rotational velocity, relative to the article, vector=R₀, and aninitial acceleration, relative to the article, vector=A₀; re-calculatingthe position vector P, velocity vector V, and orientation matrix O at asubsequent time T₁ using new information from the accelerometers andgyroscopes where: new acceleration relative to the object vector=A₁, newrotational velocity relative to the object vector=R₁,and there-calculation is carried out by, linearly interpolating from R₀ to R₁to discover the local rotational velocity at any time between T₀ and T₁,and by linearly interpolating from A₀ to A₁ to discover the localacceleration; and using the position vector P, velocity vector V, andorientation matrix O of the article to determine where a line through apredetermined one of the article's axes intersects a plane.
 14. A methodaccording to claim 13, the article further comprising three magneticsensors mounted therein, and the method further comprising: at time Testimating the position vector P, velocity vector V, and orientationmatrix O of the article, between T and an initial time T₀ where: aninitial direction of fixed magnetic field, relative to the article,vector=F₀ (in angular units); and re-calculating the position vector P,velocity vector V, and orientation matrix O at a subsequent time T₁using new information from the accelerometers, gyroscopes andmagnetometers where: a new direction of fixed magnetic field, relativeto the article, vector=F₁ (in angular units), and the re-calculation iscarried out by linearly interpolating from F₀ to F₁ to discover thelocal direction of fixed magnetic field.
 15. A method according to claim13, further comprising using ultrasonic readings to correct the velocityestimates and thereby estimated position, by modifying the historicalvelocity estimate when each new ultrasonic reading is received, to makethe estimated position match the position given by the new ultrasonicmeasurement.
 16. A method of controlling an on-screen pointer,comprising: extrapolating a position vector P, and orientation matrix Oof the article at time T, moveable from an initial state at time T₀,described by: an initial position vector=P₀, an initial velocityvector=V₀, an initial orientation matrix=O₀, (in angular units), andinitial rotational velocity, relative to the article, vector=R₀;calculating the duration of a timeslice T_(Slice)=(T−T₀)/N, where Ntends to zero; and making a rotational increment matrix M to create arotation matrix from a rotation expressed as a vector of componentsrepresenting x, v, and z axes of rotation :M=MatrixCreate(T_(Slice)*R₀), where the estimated orientation andestimated position are given as O=M₁×M₂×M₃ ×. . . ×M_(N)×O₀ andP=P₀+V(T−T₀) respectively; and using the position vector P, andorientation matrix O of the article to determine where a line through apredetermined one of the article's axes intersects a plane.
 17. A methodaccording to claim 16, further comprising using ultrasonic readings tocorrect the velocity estimates and thereby estimated position, bymodifying the historical velocity estimate when each new ultrasonicreading is received, to make the estimated position match the positiongiven by the new ultrasonic measurement.